#include "fsm_disable.h"
#include "axisgroup.h"
#include "errcode.h"
#include "log/zlog.hh"
#include "kine/kine.h"
#include "axis/axis.h"

using axisgroup::AxisGroup;
using axisgroup::FsmDisable;

#define ROB_DBG(fmt, ...) zlog()->debug("[AxisGroup{}] Disable:" fmt, grp_->get_group_id(), ##__VA_ARGS__)
#define ROB_ERR(fmt, ...) zlog()->error("[AxisGroup{}] Disable:" fmt, grp_->get_group_id(), ##__VA_ARGS__)

FsmDisable::FsmDisable(AxisGroup* g) : grp_(g) { reset(); }
void FsmDisable::reset()
{
    allow_enable_timer_ = ENABLE_MIN_TIME_MS;
    payload_check_timer_ = PAYLOAD_CHECK_TIME_MS;
    enabling_timer_ = ENABLING_TIME_MS;
    sta_has_enabled = false;
    grp_->cmd_.enable = 0;  // 从其他模式进入disable，清除掉上使能指令
}
int FsmDisable::handle_enabling()
{
    //所有轴都是standstill状态才能使能
    bool all_enabled = true;
    for (auto& joint : grp_->joints)
    {
        if (joint->run_state() != axis::Axis::RunState::Standstill)
        {
            all_enabled = false;
            break;
        }
    }
    if (all_enabled)
    {
        //使能完成开始检测负载
        if (sta_has_enabled != all_enabled)
        {
            ROB_DBG("all joints are standstill, start checking payload...");
            sta_has_enabled = true;
            payload_check_timer_ = PAYLOAD_CHECK_TIME_MS;
        }
        if (check_payload() == 0)
        {
            //负载检测通过，切换到standby状态
            grp_->change_fsm_stat(AxisGroup::FsmState::Standby);
        }
    }
    else
    {
        //使能超时检测
        if (enabling_timer_ > 0)
        {
            enabling_timer_ -= grp_->config_.cyclictime_sec * 1000;
        }
        else
        {
            ROB_ERR("enabling timeout. Reset enable cmd");
            grp_->change_to_errstat(RobEvCode::ENABLING_TIMEOUT, true);
            return -1;
        }
    }
    sta_has_enabled = all_enabled;
    return 1;
}

void FsmDisable::update(bool is_first)
{
    if (is_first)
    {
        reset();
    }

    if (grp_->is_sim())
    {
        // 仿真模式根据指令切使能状态
        if (grp_->cmd_.enable)
        {
            sta_has_enabled = true;
            grp_->change_fsm_stat(AxisGroup::FsmState::Standby);
        }
        else
        {
            sta_has_enabled = false;
        }
        return;
    }

    //若有轴处于Error状态，切换到ErrorStop状态，并关闭所有轴的使能
    for (auto& joint : grp_->joints)
    {
        if (joint->run_state() == axis::Axis::RunState::ErrorStop && grp_->cmd_.power)
        {
            grp_->change_to_errstat(RobEvCode::JOINT_ERR, true);
            break;
        }
    }

    if (allow_enable_timer_ > 0)
    {
        allow_enable_timer_ -= grp_->config_.cyclictime_sec * 1000;
        if (grp_->cmd_.enable)
        {
            grp_->change_to_errstat(RobEvCode::ENABLE_TOO_FREQUENTLY, true);
            return;
        }
    }

    if (grp_->cmd_.enable)
    {
        handle_enabling();
    }
    else
    {
        sta_has_enabled = false;
        enabling_timer_ = ENABLING_TIME_MS;
        for (auto& joint : grp_->joints) { joint->enable(false); }
    }
}

int FsmDisable::check_payload()
{
    //检测负载是否超限
    for (auto& joint : grp_->joints)
    {
        if (joint->is_torque_over())
        {
            ROB_ERR("joint {} over payload. Reset enable cmd", joint->get_id());
            grp_->change_to_errstat(RobEvCode::ENABLING_OVER_PAYLOAD, true);
            return -1;
        }
    }

    //持续检测倒计时
    if (payload_check_timer_ > 0)
    {
        payload_check_timer_ -= grp_->config_.cyclictime_sec * 1000;
        return 1;
    }

    //检测通过
    return 0;
}